#include "PID.h"

#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x)))


//PID Init
void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)
{
	pid->kp=p;
	pid->ki=i;
	pid->kd=d;
	pid->maxIntegral=maxI;
	pid->maxOutput=maxOut;
}


//PID Calc
void PID_SingleCalc(PID *pid,float reference,float feedback)
{
	//update error
	pid->lastError=pid->error;
	pid->error=reference-feedback;
	//d
	pid->output=(pid->error-pid->lastError)*pid->kd;
	//P
	pid->output+=pid->error*pid->kp;
	//I
	pid->integral+=pid->error*pid->ki;
	LIMIT(pid->integral,-pid->maxIntegral,pid->maxIntegral);//I Limit
	pid->output+=pid->integral;
	//Limit
	LIMIT(pid->output,0,pid->maxOutput);
}

//PID Clear
void PID_Clear(PID *pid)
{
	pid->error=0;
	pid->lastError=0;
	pid->integral=0;
	pid->output=0;
}
